#include <xrobot_control/xrobot_trajectory_controller.h>
#include <math.h>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <ros/duration.h>

/**
 * @brief 构造函数，初始化了ROS的节点功能
 */
TrajectoryController::TrajectoryController(std::string robot) :
 _robot(robot), _arm("manipulator"), _end("gripper")
{
    ros::NodeHandle n;

    // 订阅者初始化
   MonitorJoints_Suber = n.subscribe("xrobot_monitor/joints_control", 1, &TrajectoryController::JointsControl_Callback, this);
   MonitorEffector_Suber = n.subscribe("xrobot_monitor/effector_control", 1, &TrajectoryController::EffectorControl_Callback, this);
   VisionCmd_Suber = n.subscribe("xrobot_monitor/cmd", 1, &TrajectoryController::VisionCmd_Callback, this);
   JoyStickCmd_Suber = n.subscribe("xrobot_monitor/joystick_control", 1, &TrajectoryController::JoyStickCmd_Callback, this);

    // 发布者初始化
    Trajectory_Puber = n.advertise<trajectory_msgs::JointTrajectory>("xrobot_monitor/trajectory",10);
}

/**
 * @brief 控制机械臂某个关节运动
 * @param id 目标关节号（1-6）
 * @param  target 目标点
 * @note 目标是末端姿态
 */
void TrajectoryController::SingleJointControl(std::string id, double& _target)
{
    std::vector<double> target ;
    int index;
    // 分开机械臂和夹爪进行控制
    if(id != "Joint6")
    {
        target = _arm.getCurrentJointValues();
        if(id == "Joint1") index = 0;
        else if(id == "Joint2") index = 1;
        else if(id == "Joint3") index = 2;
        else if(id == "Joint4") index = 3;
        else if(id == "Joint5") index = 4;
        target[index] = _target;
        ArmQuicklyMoveToTarget(target);
    }
    else if(id == "Joint6")
    {
        SetGripperAngle(_target);
    }
   
    
}

/**
 * @brief 控制机械臂直线移动到目标点
 * @param target 目标点
 * @note 目标是各个关节的最终位置
 */
void TrajectoryController::ArmLinearMoveToTarget(std::vector<double>& target)
{
    // 保证关节点的数量符合要求，该机械臂为5个自由度
    assert(target.size() == JOINTS_NUM-2);
    trajectory_msgs::JointTrajectoryPoint target_point;
    target_point.positions = target;

    // 压入目标点
    Add_MidPoint(target_point);

    // 轨迹规划
    plan.trajectory_.joint_trajectory.points = Calculate_CartesianTrajectory(trajectory_points, 40);

    // 机械臂运动
    _arm.execute(plan);

    // 发布数据
    // Trajectory_Puber.publish(plan.trajectory_);

    // 清除已有过程点
    Clear_MidPoint();
}

/**
 * @brief 控制机械臂直线移动到目标点
 * @param target 目标点
 * @note 目标是末端姿态
 */
void TrajectoryController::ArmLinearMoveToTarget(geometry_msgs::Pose& _target)
{
    // 运动学逆解
    std::vector<double> target;
    target = CartesianToJointSpec(_target);

    ArmLinearMoveToTarget(target);
}

/**
 * @brief 控制机械臂快速移动到目标点
 * @param target 目标点
 * @note 目标是各个关节的最终位置
 */
void TrajectoryController::ArmQuicklyMoveToTarget(std::vector<double>& target)
{
    // 保证关节点的数量符合要求，该机械臂为5个自由度
    assert(target.size() == JOINTS_NUM-2);

    // 压入目标点
    trajectory_msgs::JointTrajectoryPoint target_point;
    target_point.positions = target;
    Add_MidPoint(target_point); 

    // 轨迹规划
    switch(TrajType)
    {
        case 0: plan.trajectory_.joint_trajectory.points = Calculate_BezierTrajectory(trajectory_points, 20); break;
        case 1: plan.trajectory_.joint_trajectory.points = Calculate_LSPBTrajectory(trajectory_points, 15); break;
        case 2: plan.trajectory_.joint_trajectory.points = Calculate_SplineTrajectory(trajectory_points, 15); break;
        case 3: plan.trajectory_.joint_trajectory.points = Calculate_SplineTrajectory(trajectory_points, 0); break;     // 用于快速响应
        default:  ROS_INFO("Error Input"); return;
    }

    ROS_INFO("Start Moving");

    // 机械臂运动
    _arm.execute(plan);

    // 发布数据
    // Trajectory_Puber.publish(plan.trajectory_);

    // 清除已有过程点
    Clear_MidPoint();
    ROS_INFO("Moving Finish");
}

/**
 * @brief 控制机械臂快速移动到目标点
 * @param target 目标点
 * @note 目标是末端姿态
 */
void TrajectoryController::ArmQuicklyMoveToTarget(geometry_msgs::Pose& _target)
{
    // 运动学逆解
    std::vector<double> target;
    target = CartesianToJointSpec(_target);

    // 设置状态
    _pose = _target;


    JointSpecToCartesian(target);

    // 控制机械臂运动
    ArmQuicklyMoveToTarget(target);
}

/**
 * @brief 关节控制回调函数
 */
void TrajectoryController::JointsControl_Callback(sensor_msgs::JointState msg)
{
    // 设置轨迹规划类型
    TrajType = msg.velocity[0];

    if(msg.name[0] == "AddMidPonts")
    {
        msg.position.pop_back();
        Add_MidPoint(msg);
    }

    else if(msg.name[0] == "Joints")
    {
        double end_target = msg.position.back();
        msg.position.pop_back();
        ArmQuicklyMoveToTarget(msg.position);
        SetGripperAngle(end_target);
    }
    else 
    {
        SingleJointControl(msg.name[0], msg.position[0]);
    }
}

/**
 * @brief 末端控制回调函数
 * @param msg  参数列表：msg.orientation.z ： 轨迹规划曲线类型
 *                                                     msg.orientation.w ： 轨迹规划运动类型
 *                                                     msg.position.x/y/z ： 目标点
 */
void TrajectoryController::EffectorControl_Callback(geometry_msgs::Pose msg)
{
    // 设置轨迹规划类型
    TrajType = msg.orientation.z;

    switch((int)msg.orientation.w)
    {
        case 0: 
                ArmQuicklyMoveToTarget(msg);
                break;
        case 1: 
                ArmLinearMoveToTarget(msg);
                break;
        case 2: 
               Add_MidPoint(msg);
                break;
        default:
                break;
    }
}

/**
 * @brief 视觉指令回调函数
 * 
 * @param msg 上位机指令
 * 参数列表：msg.orientation.w 指令类型
 *                        msg.orientation.z  特定命令的指令参数
 */
void TrajectoryController::VisionCmd_Callback(geometry_msgs::Pose msg)
{
    // 准备姿态
    if(!msg.orientation.z) Vision_Waiting((int)msg.orientation.w);

    // 判断视觉指令类型
    else
    { 
        switch ((int)msg.orientation.w)
        {
            case  1: 
                Vision_ShapeMove(msg.orientation.z);
                break;
            case  2: 
                Vision_HandTask(msg);
                break;
            case  3: 
                Vision_GripperTask(msg);
                break;
        default:
            break;
        }
    }

}

/**
 * @brief 视觉指令回调函数
 * 
 * @param msg 上位机指令
 * 参数列表：msg.orientation.w 指令类型
 *                        msg.orientation.z  特定命令的指令参数
 */
void TrajectoryController::JoyStickCmd_Callback(geometry_msgs::Pose msg)
{
    std::vector<double> js;
    geometry_msgs::Pose pose;
    switch ((int)msg.orientation.w)
    {
        case 1: 
        
            TrajType = 3;
            pose = this->_pose;
            if(msg.position.x > 0)  pose.position.x += 40;
            else if(msg.position.x < 0) pose.position.x -= 40;
            if(msg.position.y > 0)  pose.position.y += 40;
            else if(msg.position.y < 0) pose.position.y -= 40;
            ArmQuicklyMoveToTarget(pose);
            break;

        case 2: 
            TrajType = 3;
            pose = this->_pose;
            if(msg.position.z == 0)  pose.position.z += 30;
            else if(msg.position.z == 1) pose.position.z -= 30;
            ArmQuicklyMoveToTarget(pose);
            break;       

        break;
        case 3: 
            if(!msg.position.z) SetGripperStatus(1);
            else SetGripperStatus(2);
            break;
        default: break;
    }
}

/**
 * @brief 机械臂移动到视觉等待位姿
 * 
 * @param type 
 */
void TrajectoryController::Vision_Waiting(int type)
{
    geometry_msgs::Pose pose;
    switch (type)
    {
    case 1: 
        pose.position.x = 100;  pose.position.y = 0;  pose.position.z = 30;  pose.orientation.y = 1.57; pose.orientation.x= 0; 
        break;
    case 2: 
        pose.position.x = 100;  pose.position.y = 0;  pose.position.z = 30;  pose.orientation.y = 1.57; pose.orientation.x= 0; 
        break;
    case 3: 
        pose.position.x = 250;  pose.position.y = 0;  pose.position.z = 200;  pose.orientation.y = 0; pose.orientation.x= 0; 
        break;    
    default:
        break;
    }

    // 更新状态
    _pose = pose;

    std::vector<double>  js = CartesianToJointSpec(pose);
    js[3] -= pitch_bias;
    ArmQuicklyMoveToTarget(js);
    SetGripperStatus(0);
}

/**
 * @brief 视觉夹取任务
 * 
 * @param cmd 指令
 */
void TrajectoryController::Vision_GripperTask(geometry_msgs::Pose cmd)
{
    // 打开夹爪
    SetGripperStatus(2);
    
    // 移动到目标点
    geometry_msgs::Pose target = CameraToWorld(cmd);
    TrajType = 2;
     geometry_msgs::Pose mid = target; mid.position.z = 20;
     Add_MidPoint(mid);
    ArmQuicklyMoveToTarget(target);

    // 夹取
    ros::Duration(1).sleep(); 
    SetGripperStatus(1);
    ros::Duration(1).sleep(); 

    // 移动指定位置
    target = Get_Colorbox_Displayment(cmd.orientation.z);
    ArmQuicklyMoveToTarget(target); 

    // 下降
    target.position.z = -30;
    ArmQuicklyMoveToTarget(target);

    // 松开夹爪 
    SetGripperStatus(2); 

    // 上升
    target.position.z = 0;
    ArmQuicklyMoveToTarget(target);  

    // 回到起始等待位置
    Vision_Waiting(3);
}

/**
 * @brief 视觉手势识别任务
 * 
 * @param cmd 
 */
void TrajectoryController::Vision_HandTask(geometry_msgs::Pose cmd)
{
    double dx = cmd.position.x;
    double dy = cmd.position.y;
    double gripper = cmd.position.z;
    geometry_msgs::Pose pose;
    pose.position.x = 250;  pose.position.y = 0;  pose.orientation.y = 0; pose.orientation.x= 0; 
    TrajType = 3;

    // 指令
    switch ((int)gripper)
    {
        case 5: SetGripperStatus(1); break;
        case 6: SetGripperStatus(2); break;
        case 7: 
            pose.position.z = 200;   ArmQuicklyMoveToTarget(pose);
            pose.position.z = 100;   ArmQuicklyMoveToTarget(pose);
            pose.position.z = 200;   ArmQuicklyMoveToTarget(pose);
            pose.position.z = 100;   ArmQuicklyMoveToTarget(pose);
            ros::Duration(2).sleep();
            Vision_Waiting(2);
            break;
        default:   
            // 跟踪
            std::vector<double> js = _arm.getCurrentJointValues();
            js[3] = Restriction(js[3]+dy*0.001, -3.14, 3.14);
            js[0] = Restriction(js[0]-dx*0.0015, -1.57, 1.57);
            ArmQuicklyMoveToTarget(js);
            break;
    }
}

/**
 * @brief 视觉随形状移动任务
 * 
 * @param cmd 指令
 */
void TrajectoryController::Vision_ShapeMove(int shape)
{
    geometry_msgs::Pose target;
    trajectory_msgs::JointTrajectoryPoint p;
    const double POINT_NUM = 100.;
    l3 -= gripper_len;

    // 圆形运动半径
    double r = 80.;

    // 确定运动平面
    target.position.x = 50;   

    // 按照形状移动
    switch (shape)
    {
        // 三角形
        case 3:  
             target.position.y = 0;     target.position.z = 150;    
            ArmQuicklyMoveToTarget(target);
            target.position.y = 0;     target.position.z = 200;    
            Add_MidPoint(target);
            target.position.y = 100;     target.position.z = 100; 
            Add_MidPoint(target);
            target.position.y = -100;     target.position.z = 100; 
            Add_MidPoint(target);
            target.position.y = 0;     target.position.z = 200; 
            ArmLinearMoveToTarget(target);
            break;

        // 矩形
        case  4:  
            target.position.y = 0;     target.position.z = 150;    
            ArmQuicklyMoveToTarget(target);
            target.position.y = -50;     target.position.z = 200;    
            ArmLinearMoveToTarget(target);
            ros::Duration(1).sleep();
            target.position.y = 50;     target.position.z = 200; 
            ArmLinearMoveToTarget(target);
            ros::Duration(1).sleep();
            target.position.y = 50;     target.position.z = 100; 
            ArmLinearMoveToTarget(target);
            ros::Duration(1).sleep();
            target.position.y = -50;     target.position.z = 100; 
            ArmLinearMoveToTarget(target);
            ros::Duration(1).sleep();
            target.position.y = -50;     target.position.z = 200;  
            ArmLinearMoveToTarget(target);
            break; 

        // 圆形
        case  5:  
            target.position.y = r;     target.position.z = 100;    
            ArmQuicklyMoveToTarget(target);
            plan.trajectory_.joint_trajectory.points.clear();
            for(int i = 0; i <= POINT_NUM; i ++)
            {
                    target.position.y = r * cos(i / POINT_NUM * 2. * 3.1415);  
                    target.position.z = 100 + r * sin(i / POINT_NUM * 2. * 3.1415); 
                    p.positions = CartesianToJointSpec(target);
                    plan.trajectory_.joint_trajectory.points.push_back(p);
            }
            _arm.execute(plan);
            ros::Duration(4).sleep();
            break;

        default: break;
    }
    l3 += gripper_len;
    Vision_Waiting(1);
}


int main(int argc, char* argv[])
{
    //ROS节点初始化
    ros::init(argc, argv, "trajectory_controller");

    //创建控制实体
    TrajectoryController controller("xrobot");

    //开始运行move_group
    controller.Control();
    return 0;
}

/**
 * @brief 笛卡尔空间到关节空间的解算(逆运动学)
 */
std::vector<double> TrajectoryController::CartesianToJointSpec(double x, double y, double z, double pitch, double roll)
{
    std::vector<double> js;
    double theta, _alpha, _beta, _gamma, _fhi, _l, _lx;
    double test;
    // 求解中间量
    _alpha = atan2(sqrt(pow(x,2)+pow(y,2)), z);
    _l = sqrt(pow(x,2)+pow(y,2)+pow(z,2));
    _beta = M_PI/2. - pitch -_alpha;
    _lx = sqrt(pow(l3,2)+pow(_l,2) - 2*l3*_l*cos(_beta));
    test = pow(l1,2) + pow(_lx,2) - pow(l2,2);
    test = 2. * l1 * _lx;
    test = (pow(l1,2) + pow(_lx,2) - pow(l2,2)) / (2. * l1 * _lx);
    _fhi = acos(TC((pow(l1,2) + pow(_lx,2) - pow(l2,2)) / (2. * l1 * _lx)));
    _gamma = acos(TC((pow(_lx,2) + pow(_l,2) - pow(l3,2.)) / (2. * _lx * _l)));

    // theta1
    theta = atan2(y, x);
    js.push_back(theta);

    // theta2
    theta = _alpha - _gamma -_fhi;
    js.push_back(theta);

    // theta3
    theta = M_PI - acos(TC((pow(l1,2) + pow(l2,2) - pow(_lx,2)) / (2 * l1 * l2)));
    js.push_back(theta);

    // theta4 
    theta = _beta + _gamma + _fhi  - theta;
    js.push_back(theta);

    // theta5
    theta = roll;
    js.push_back(theta);    

    return js;
}

/**
 * @brief 关节空间到笛卡尔空间的结算(正运动学)
 * 
 * @param js 
 * @return geometry_msgs::Pose 
 */
geometry_msgs::Pose TrajectoryController::JointSpecToCartesian( std::vector<double> js)
{
    geometry_msgs::Pose pose;
    double hlen = l1 * sin(js[1]) + l2 * sin(js[1] + js[2]) + l3 * sin(js[1] + js[2] + js[3]);
    pose.position.x = cos(js[0]) * hlen;
    pose.position.y = sin(js[0]) * hlen;
    pose.position.z =  l1 * cos(js[1]) + l2 * cos(js[1] + js[2]) + l3 * cos(js[1] + js[2] + js[3]);
    pose.orientation.x = js[4];
    pose.orientation.y = 1.571 - js[1] - js[2] - js[3];
    return pose;
}

/**
 * @brief 将相机坐标系下的点转为世界坐标系
 * 
 * @param msg 相机坐标系下的目标点
 * @return geometry_msgs::Pose  世界坐标系下的目标点
 */
geometry_msgs::Pose  TrajectoryController::CameraToWorld(geometry_msgs::Pose msg)
{
    using namespace Eigen;

    // 长度参数
    static const double h = 80;

    // 计算当前末端坐标
    l3 -= gripper_len;
    std::vector<double> cur_js = _arm.getCurrentJointValues(); cur_js[3] += pitch_bias;
    geometry_msgs::Pose cur_pose = JointSpecToCartesian(cur_js);
    l3 += gripper_len;

    // 获取相机坐标
    geometry_msgs::Pose camera_pose;
    double theta = atan2(cur_pose.position.y, cur_pose.position.x);
    double pitch = cur_pose.orientation.y;
    camera_pose.position.x = cur_pose.position.x + h * sin(pitch) * cos(theta);
    camera_pose.position.y = cur_pose.position.y + h * sin(pitch) * sin(theta);
    camera_pose.position.z = cur_pose.position.z - h * cos(pitch);
    camera_pose.orientation.y = pitch- 1.57;

    // 获取目标点
    geometry_msgs::Pose target;
    target.position.x = camera_pose.position.x - msg.position.y + 65;   // 最后的值为补偿值
    target.position.y = camera_pose.position.y+ msg.position.x - 55 ;   // 最后的值为补偿值;
    target.position.z = -45;
    target.orientation.y = -1;
    target.orientation.x = 0;
    return target;

}

/**
 * @brief  计算贝塞尔曲线
 * @param points  控制点（包括起始点和终点）
 * @param target_num  两个点之间生成轨迹的点数，最终会包含起点，即返回的点数为（target_num+1）
 * @return std::vector<trajectory_msgs::JointTrajectoryPoint>  生成的过程点
 */
std::vector<trajectory_msgs::JointTrajectoryPoint> 
TrajectoryController::Calculate_BezierTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int target_num)
{
    int poinets_num = points.size();                                 // 过程点数目   
    double dt = 1 / (double)target_num;                       // 过程点时间间隔
    int joints_num = points[0].positions.size();         // 关节数目
    trajectory_msgs::JointTrajectoryPoint point;
    double joint_val;
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory;

    // 压入起始点
    points[0].positions = _arm.getCurrentJointValues();
    trajectory.push_back(points[0]);

    // 调用贝塞尔曲线公式
    for(int i = 1; i <=  target_num; i ++)
    {
        point.positions.clear();
        for(int j = 0; j < joints_num; j++)
        {  
            joint_val = 0;
            for(int k = 0; k < poinets_num; k ++)
            {
                joint_val += Calculate_CombinatorialNumber(poinets_num-1, k) * points[k].positions[j] * pow((1-dt*i), poinets_num-1-k) * pow(dt*i, k);
            }
            point.positions.push_back(joint_val);
        }
        trajectory.push_back(point);
    }
    return trajectory;
}

/**
 * @brief  计算抛物线-直线-抛物线( LSPB )轨迹
 * 
 * @param points 中间点（包括起始点和终点）
 * @param diff_num 两个点之间的点个数
 * @return std::vector<trajectory_msgs::JointTrajectoryPoint> 生成的过程点
 */
std::vector<trajectory_msgs::JointTrajectoryPoint> 
TrajectoryController::Calculate_LSPBTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num)
{
    int points_num = points.size();
    double joint_val, a_;

    // 获取过程点个数，n+1个点
    int len = points.size();

    // 获取关节数目
    int jn = points[0].positions.size();

    // 修改起始点
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory(diff_num*(len-1) + len);
    points[0].positions = _arm.getCurrentJointValues();
    trajectory[0] = points[0];   // 起始点

    // 定义抛物线加速度和两点时间间隔，为了方便，这里统一定义为定值
    std::vector<double> acc(len);
    std::vector<double> deltaT(len - 1);
    for(int i = 0; i < len; i ++) 
    {
        acc[i] = 150.;
        if(i != len -1) deltaT[i] = 1.;
    }
    // deltaT[0] = 2; deltaT[1] = 1; deltaT[2] = 3; 

    // 存储数据
    std::vector<double> Vd(len-1);  // 两点间的直线速度
    std::vector<double> Td(len-1);  // 两点间直线部分的时间
    std::vector<double> Ti(len);        // 每个点附近的时间

    double dp;
    // 遍历每个关节
    for(int j = 0; j < jn; j ++)
    {
        // 只有一个点，直接返回
        if(len == 1) return trajectory;

        // 不带中间点
        else if(len == 2) 
        {
            dp = points[1].positions[j] - points[0].positions[j];
            a_ = sign(dp)*abs(acc[0]);
            assert(a_ >= 4 * abs(dp / pow(deltaT[0], 2)));
            Ti[0] = 0.5 * deltaT[0] - sqrt((pow(a_ * deltaT[0], 2) - 4 * a_ * dp)) / (2 * a_);
            Ti[1] = Ti[0];
            Vd[0] = a_ * Ti[0];
            Td[0] = deltaT[0] - 2 * Ti[0];
        }

        // 带中间点
        else
        {
            // 计算起始的时间和速度
            dp = points[1].positions[j] - points[0].positions[j];
            a_ = sign(dp)*abs(acc[0]);
            Ti[0] = deltaT[0] -  sqrt(pow(deltaT[0],2) - 2. * dp / a_);
            Vd[0] = dp / (deltaT[0] - 0.5 * Ti[0]);   

            // 计算中间点的速度
            for(int i = 1;  i < len - 2; i++ )
            {
                dp = points[i+1].positions[j] - points[i].positions[j];
                Vd[i] = dp / deltaT[i];
            }

            // 计算结束段的时间和速度
            dp = points[len-1].positions[j] - points[len-2].positions[j];
            a_ = sign(-dp)*abs(acc[len-1]);
            Ti[len-1] = deltaT[len-2] - sqrt(pow(deltaT[len-2],2) + 2. * dp / a_);
            Vd[len-2] = dp / (deltaT[len-2] - 0.5 * Ti[len-1]);         
            

            // 计算中间点时间
            for(int i = 1;  i < len - 1; i++ )
            {
                a_ = sign(Vd[i] - Vd[i-1]) * abs(acc[i]);
                Ti[i] = (Vd[i] - Vd[i-1]) / a_; 
            }

            // 计算中间两点时间间隔
            for(int i = 1;  i < len - 2; i++ )
            {
                Td[i] = deltaT[i] - 0.5 * Ti[i] - 0.5 * Ti[i+1];
            }

            // 数据补充
            Td[len-2]  = deltaT[len-2] - Ti[len-1] - 0.5 * Ti[len-2];       
            Td[0] = deltaT[0] - Ti[0] - 0.5 * Ti[1];      
        }
        
        // 取离散点作为轨迹点
        double dt, t_;

        for(int i = 0; i < len - 1; i ++)
        {
            // 两点间轨迹点的时间间隔
            dt =  deltaT[i] / (diff_num + 1);

            //  始末点
            double start = points[i].positions[j];
            double end = points[i+1].positions[j];

            for(int k = 1; k <= diff_num+1; k ++)
            {
                // 根据时间所在区域进行取值
                t_ =  k * dt;

                // 判断起始点或者终点端
                double Tb1 = (i == 0) ? Ti[i] : Ti[i] / 2.;
                double Tm = Tb1 + Td[i];
                double Tb2 = Tm + (i == len - 2) ? Ti[i+1] : Ti[i+1] / 2.;
    
                if(t_ <= Tb1) joint_val =  start + 0.5 * acc[i] * pow(t_, 2);
                else if(t_ > Tb1 && t_ < Tm) joint_val = start + 0.5 * acc[i] * pow(Ti[i], 2) + Vd[i] * (t_ - Tb1);
                else joint_val = start + 0.5 * acc[i] * pow(Ti[i],  2) + Vd[i] * Td[i] + Vd[i] *(t_ - Tm) - 0.5 * acc[i] * pow((t_ - Tm), 2);

                // 输入到轨迹中
                trajectory[i * (diff_num+1) + k].positions.push_back(joint_val);
            }
        }
    }   
    return trajectory;
}

/**
 * @brief 计算三次样条曲线
 * 
 * @param points 中间点（包括起始点和终点）
 * @param diff_num 两个点之间的点个数
 * @return std::vector<trajectory_msgs::JointTrajectoryPoint> 
 */
std::vector<trajectory_msgs::JointTrajectoryPoint> 
TrajectoryController::Calculate_SplineTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num)
{
    using namespace Eigen;

    int points_num = points.size();
    double joint_val, a_;

    // 获取过程点个数，n+1个点
    int len = points.size();

    // 获取关节数目
    int jn = points[0].positions.size();

    // 修改起始点
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory(diff_num*(len-1) + len);
    points[0].positions = _arm.getCurrentJointValues();
    trajectory[0] = points[0];   // 起始点

    // 时间间隔，为了方便计算，这里统一设值
    std::vector<double> dT(len - 1);
   for(int i = 0; i < len - 1; i ++) dT[i] = 1;

    const int M_SIZE = 4 * (len - 1);

    // 变量矩阵
    MatrixXd A;  A.setZero(M_SIZE, M_SIZE);  
    VectorXd X(M_SIZE);
    VectorXd b(M_SIZE);
    VectorXd solve(M_SIZE);

    // 遍历关节
    for(int j = 0; j < jn; j ++)
    {
        int index = 0;
        // 设置方程参数

        // 始末速度为0   
        A(index,  1) = 1;   b(index) = 0;
        index ++;

        A(index, 4 * (len - 2) + 1) = 1;  A(index, 4 * (len - 2) + 2) = 2 * dT[len - 2];
        A(index, 4 * (len - 2) + 3) = 3 * pow(dT[len - 2], 2);
        b(index) = 0;
        index ++;
        for(int  i = 0; i < len - 1; i ++)
        {
            // 位置
            A(index,  i*4 + 0) = 1; b(index) = points[i].positions[j];
            index ++;
            A(index, i*4 + 0) = 1;      A(index, i*4 + 1) = dT[i];      
            A(index, i*4 + 2) = pow(dT[i], 2);      A(index, i*4 + 3) = pow(dT[i], 3);      
            b(index) = points[i+1].positions[j];
            index ++;

            // 速度
            if(i != 0)
            {
                A(index, (i-1)*4+1) = 1;  A(index, (i-1)*4+2) = 2 * dT[i-1]; 
                A(index, (i-1)*4+3) = 3* pow(dT[i-1], 2);   A(index, i*4+1) = -1;
                b(index) = 0;
                index ++;
            }
            // 加速度
            if(i != 0)
            {
                A(index, (i-1)*4+2) = 2; A(index, (i-1)*4+3) = 6 * dT[i-1]; 
                A(index, i*4+2) = -2;
                b(index) = 0; 
                index ++;
            }
        }
        // std::cout << A << std::endl;
        // std::cout << b << std::endl;

        // 求解方程
        // X = A.llt().solve(b);
        X = A.colPivHouseholderQr().solve(b);
        // std::cout << X << std::endl;

        // 取离散点作为轨迹点
        double dt, t_;

        for(int i = 0; i < len - 1; i ++)
        {
            // 两点间轨迹点的时间间隔
            dt =  dT[i] / (diff_num + 1);

            for(int k = 1; k <= diff_num+1; k ++)
            {
                // 代入三次多项式公式
                t_ =  k * dt;
                joint_val = X(i*4) + X(i*4+1) * t_ + X(i*4+2) * pow(t_, 2) +  X(i*4+3) * pow(t_, 3);

                // 输入到轨迹中
                trajectory[i * (diff_num+1) + k].positions.push_back(joint_val);
            }
        }
    }
    return trajectory;
}

/**
 * @brief 计算笛卡尔空间下的轨迹
 * 
 * @param points 中间点（包括起始点和终点）
 * @param diff_num 两个点之间的点个数
 * @return std::vector<trajectory_msgs::JointTrajectoryPoint> 
 */
std::vector<trajectory_msgs::JointTrajectoryPoint> 
TrajectoryController::Calculate_CartesianTrajectory(std::vector<trajectory_msgs::JointTrajectoryPoint> points, int diff_num)
{
    int points_num = points.size();
    std::vector<geometry_msgs::Pose> pose_points;
    geometry_msgs::Pose pose_point, detla_pose;
 
    trajectory_msgs::JointTrajectoryPoint joint_point;
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory;

    // 压入起始点
    points[0].positions = _arm.getCurrentJointValues();
    trajectory.push_back(points[0]);

    // 关节空间转为笛卡尔空间
    for(int i = 0; i < points_num; i ++) pose_points.push_back(JointSpecToCartesian(points[i].positions));

    // 计算过程点
    for(int i = 0; i < points_num - 1; i ++)
    {
        // 计算两点间的单位间隔
        detla_pose.position.x = (pose_points[i+1].position.x - pose_points[i].position.x) / (diff_num + 1);
        detla_pose.position.y = (pose_points[i+1].position.y - pose_points[i].position.y) / (diff_num + 1);
        detla_pose.position.z = (pose_points[i+1].position.z - pose_points[i].position.z) / (diff_num + 1);
        for(int j = 1; j <= diff_num+1; j ++)
        {
            pose_point.position.x = pose_points[i].position.x + detla_pose.position.x * j;
            pose_point.position.y = pose_points[i].position.y + detla_pose.position.y * j;
            pose_point.position.z = pose_points[i].position.z + detla_pose.position.z * j;

            // 笛卡尔空间到关节空间
            joint_point.positions = CartesianToJointSpec(pose_point);
            trajectory.push_back(joint_point);
        }
    }
    return trajectory;
}


